/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

import edu.wpi.first.wpilibj.networktables.NetworkTable;


/**
 *
 * @author Programming
 */
public class Target {
    public enum Type {
        LEFT (0) , MIDDLE (1), RIGHT(2), LOR(3); // LOR = LEFT OR RIGHT
        public int value;
        public int getValue()
        {
            return value;
        }
        private Type(int _value){
            this.value = _value;
        }
    };
    
    public Type type;
    public Blob blobInner, blobOuter;
    public double aspectRatio;
    public double height;
    public double elevation;
    public double cameraAngle;
    public double centerInnerX, centerOuterX;
    public double centerInnerY, centerOuterY;
    public double distance = 0;
    public double targetAngleWidth = 0;
    public double azimuth = 0;
    public double distanceToRobot, angleToRobot;
    public boolean initalized;
    
    static final double verticalFOV = 35 * Math.PI/180;
    static final double horizontalFOV = 1.4985;
    static final double cameraHorizontalOffset = 0;
    static final double cameraHeight = 31.5;
    static final double cameraVerticalAngle = -11/*-9.8 */ * Math.PI/180;
    static final double cameraHorizontalAngle = 0 *  Math.PI/180;
    static final double imageWidth = 640;
    static final double imageHeight = 480;
    
    static final double ratioThreshold = 1.2444444;
    static final double centerThreshold = 9;
    static final double areaThreshold = .2;
    
    static final double targetLORElevation = 225.1; //distance the bottom is off the cround
    static final double targetMiddleElevation = 264.5;
    static final double targetLORHeight = 53.3;       ///from base to top of target
    static final double targetMiddleHeight = 30.5;
    static final double targetWidth = 137.16;
    static final double targetMiddleX = 0;
    static final double targetLeftX = -45.974 - targetWidth;
    static final double targetRightX = 45.974 + targetWidth;//set these
    static final double targetMiddleAspectRatio = 5 ;
    static final double targetLORAspectRatio = 2.5;




    public Target(Type _type){
        type = _type;
        if (type == Type.LEFT || type == Type.RIGHT || type == Type.LOR ){
            height = targetLORHeight;
            elevation = targetLORElevation;
            aspectRatio = targetLORAspectRatio;    
        }
        if (type == Type.MIDDLE){
            height = targetMiddleHeight;
            elevation = targetMiddleElevation;
            aspectRatio = targetMiddleAspectRatio;    
        }
    }

    public void doMath(NetworkTable table){
        blobOuter.findCM();

       double left,right,top,bottom;
       left = Math.min(Math.min(blobOuter.x1,blobOuter.x2),Math.min(blobOuter.x3,blobOuter.x4));
       right = Math.max(Math.max(blobOuter.x1,blobOuter.x2),Math.max(blobOuter.x3,blobOuter.x4));
       bottom = Math.min(Math.min(blobOuter.y1,blobOuter.y2),Math.min(blobOuter.y3,blobOuter.y4));
       top = Math.max(Math.max(blobOuter.y1,blobOuter.y2),Math.max(blobOuter.y3,blobOuter.y4));


        distance = blobOuter.getDist(Blob.getTargetElevation(type),blobOuter.getVertAngle(blobOuter.cmx, (top + bottom)/2));
        targetAngleWidth = blobOuter.getHorizAngle(right, blobOuter.cmy) - blobOuter.getHorizAngle(left, blobOuter.cmy);
        azimuth = blobOuter.getAzimuthFromWidth(targetAngleWidth * 180/Math.PI,distance);
        if (right > left)
            azimuth *= -1;

        System.out.println(this.type.name() + ":" + blobOuter.cmx/this.imageWidth + ": " + Math.floor((distance)/2.54) );
        table.putNumber("Distance to Target", distance);
        if(type == Type.MIDDLE)
        table.putNumber("X Error", blobOuter.cmx/this.imageWidth);


        cameraAngle = blobOuter.getHorizAngle((left + right)/2,blobOuter.cmy);
    }


    public void clear(){
        initalized = false;
    }

    public boolean inView(){
        return initalized;
    }

    public void setBlob(daBlob _daBlob,NetworkTable table){
        blobInner = _daBlob.blobInner;
        blobOuter = _daBlob.blobOuter;
        initalized = true;
      //  blobOuter = _blobOuter;
        double[] tempI = blobInner.getCenter();
        double[] tempO = blobOuter.getCenter();
        centerInnerX = tempI[0];
        centerInnerY = tempI[0];
        centerOuterX = tempO[0];
        centerOuterY = tempO[1];
        aspectRatio = blobInner.getAspectRatio();
        this.doMath(table);
    }

    public void Update( double _distanceToRobot,double _angleToRobot){
            distanceToRobot = _distanceToRobot;
            angleToRobot = _angleToRobot;
    }
    

    public static double getTargetX(Type _targPos) {
	switch(_targPos) {
	case RIGHT:
             return targetRightX;
        case LEFT:
             return targetLeftX;
	case MIDDLE:
             return targetMiddleX;
        case LOR:
            return 0.0;
	default:
                System.out.println("Take a look at Target.java in function GetTargetX");
		return 0.0;
	}
        
}
}
